#include "bsp_mc_PID_regulators.h"
#include "bsp_mc_globals.h"
#include "bsp_mc_foc_driver.h"
#include "bsp_mc_hall.h"
#include "stdlib.h"



s16 curqtemp=0,curdtemp=0;



void PID_Init (PID_Struct_t *PID_Torque, PID_Struct_t *PID_Flux, PID_Struct_t *PID_Speed)
{

	hTorque_Reference = PID_TORQUE_REFERENCE;

	PID_Torque->hKp_Gain    = PID_TORQUE_KP_DEFAULT;
	PID_Torque->hKp_Divisor = TF_KPDIV;  

	PID_Torque->hKi_Gain = PID_TORQUE_KI_DEFAULT;
	PID_Torque->hKi_Divisor = TF_KIDIV;

	PID_Torque->hKd_Gain = PID_TORQUE_KD_DEFAULT;
	PID_Torque->hKd_Divisor = TF_KDDIV;
	PID_Torque->wPreviousError = 0;

	PID_Torque->hLower_Limit_Output=S16_MIN;   //Lower Limit for Output limitation
	PID_Torque->hUpper_Limit_Output= S16_MAX;   //Upper Limit for Output limitation
	PID_Torque->wLower_Limit_Integral = S16_MIN * TF_KIDIV;
	PID_Torque->wUpper_Limit_Integral = S16_MAX * TF_KIDIV;
	PID_Torque->wIntegral = 0;

	/**************************************************/
	/************END PID Torque Regulator members*******/
	/**************************************************/

	/**************************************************/
	/************PID Flux Regulator members*************/
	/**************************************************/
	hFlux_Reference = PID_FLUX_REFERENCE;

	PID_Flux->hKp_Gain    = PID_FLUX_KP_DEFAULT;
	PID_Flux->hKp_Divisor = TF_KPDIV;  

	PID_Flux->hKi_Gain = PID_FLUX_KI_DEFAULT;
	PID_Flux->hKi_Divisor = TF_KIDIV;

	PID_Flux->hKd_Gain = PID_FLUX_KD_DEFAULT;
	PID_Flux->hKd_Divisor = TF_KDDIV;
	PID_Flux->wPreviousError = 0;

	PID_Flux->hLower_Limit_Output=S16_MIN;   //Lower Limit for Output limitation
	PID_Flux->hUpper_Limit_Output= S16_MAX;   //Upper Limit for Output limitation
	PID_Flux->wLower_Limit_Integral = S16_MIN * TF_KIDIV;
	PID_Flux->wUpper_Limit_Integral = S16_MAX * TF_KIDIV;
	PID_Flux->wIntegral = 0;



	PID_Speed->wIntegral = 0;  // reset integral value 

//	hSpeed_Reference = PID_SPEED_REFERENCE_RPM;

	PID_Speed->hKp_Gain    = PID_SPEED_KP_DEFAULT;
	PID_Speed->hKp_GainLowspeed = PID_LOWSPEED_KP_DEFAULT;
	PID_Speed->hKp_Divisor = SP_KPDIV;  

	PID_Speed->hKi_Gain = PID_SPEED_KI_DEFAULT;
	PID_Speed->hKi_Divisor = SP_KIDIV;

	PID_Speed->hKd_Gain = PID_SPEED_KD_DEFAULT;
	PID_Speed->hKd_Divisor = SP_KDDIV;
	PID_Speed->wPreviousError = 0;

	PID_Speed->hLower_Limit_Output= -IQMIN;   //Lower Limit for Output limitation
	PID_Speed->hUpper_Limit_Output= IQMAX;   //Upper Limit for Output limitation
	PID_Speed->wLower_Limit_Integral = -IQMIN * SP_KIDIV;
	PID_Speed->wUpper_Limit_Integral = IQMAX * SP_KIDIV;
	PID_Speed->wIntegral = 0;

}



  s16 PI_Regulator(s16 hReference, s16 hPresentFeedback, PID_Struct_t *PID_Struct)
{

	s32 wError,wProportional_Term,wIntegral_Term,houtput_32;
	s64 dwAux; 
	// error computation
	wError= (s32)(hReference - hPresentFeedback);

	// Proportional term computation
	wProportional_Term = PID_Struct->hKp_Gain * wError;

	// Integral term computation
	if (PID_Struct->hKi_Gain == 0)
	{
		PID_Struct->wIntegral = 0;
	}
	else
	{ 
		wIntegral_Term = PID_Struct->hKi_Gain * wError;
		dwAux = PID_Struct->wIntegral +(s64)(wIntegral_Term);


		if (dwAux > PID_Struct->wUpper_Limit_Integral)
		{
		  PID_Struct->wIntegral = PID_Struct->wUpper_Limit_Integral;
		}
		else if (dwAux < PID_Struct->wLower_Limit_Integral)
			  { 
				PID_Struct->wIntegral = PID_Struct->wLower_Limit_Integral;
			  }
			  else
			  {
			   PID_Struct->wIntegral = (s32)dwAux;
			  }

	}
	
	houtput_32 = (wProportional_Term/PID_Struct->hKp_Divisor+ 
                PID_Struct->wIntegral/PID_Struct->hKi_Divisor);
	
	if (houtput_32 >= PID_Struct->hUpper_Limit_Output)
	{
		return(PID_Struct->hUpper_Limit_Output);		  			 	
	}
	else if (houtput_32 < PID_Struct->hLower_Limit_Output)
	{
		return(PID_Struct->hLower_Limit_Output);
	}
	else 
	{
		return((s16)(houtput_32)); 		
	}
}



s16 Speed_PI_Regulator(s16 hReference, s16 hPresentFeedback, PID_Struct_t *PID_Struct)
{

	   s32 wError,wProportional_Term,wIntegral_Term,houtput_32;
	   s64 dwAux; 
    // error computation
       wError = (s32)(hReference - hPresentFeedback);

    // Proportional term computation
	wProportional_Term = PID_Struct->hKp_Gain * wError;
		
    // Integral term computation
    if (PID_Struct->hKi_Gain == 0)
		{
        PID_Struct->wIntegral = 0;
    }
    else
		{
		wIntegral_Term = PID_Struct->hKi_Gain * wError;
		dwAux = PID_Struct->wIntegral + (s64)(wIntegral_Term);

        if (dwAux > PID_Struct->wUpper_Limit_Integral)
        {
            PID_Struct->wIntegral = PID_Struct->wUpper_Limit_Integral;
        }
        else if (dwAux < PID_Struct->wLower_Limit_Integral) {
            PID_Struct->wIntegral = PID_Struct->wLower_Limit_Integral;
        }
        else {
            PID_Struct->wIntegral = (s32)(dwAux);
        }
    }

    houtput_32 = (wProportional_Term / PID_Struct->hKp_Divisor + \
                  PID_Struct->wIntegral / PID_Struct->hKi_Divisor);

    if (houtput_32 >= PID_Struct->hUpper_Limit_Output) {
        return(PID_Struct->hUpper_Limit_Output);
    }
    else if (houtput_32 < PID_Struct->hLower_Limit_Output) {
        return(PID_Struct->hLower_Limit_Output);
    }
    else {
        return((s16)(houtput_32));
    }


}





void FOC_CalcFluxTorqueRef(float rpmset,float rpmfb)
{
	hSpeed_Reference=(s16)rpmset;
	curqtemp= Speed_PI_Regulator(hSpeed_Reference,(s16)rpmfb,&PID_Speed_InitStructure);
	if(curqtemp>Stat_Curr_q_d_ref.qI_Component1)
	{
		Stat_Curr_q_d_ref.qI_Component1=Stat_Curr_q_d_ref.qI_Component1+500;
		if(Stat_Curr_q_d_ref.qI_Component1>curqtemp)
		{Stat_Curr_q_d_ref.qI_Component1=curqtemp;}
	}
	if(curqtemp<Stat_Curr_q_d_ref.qI_Component1)
	{
		Stat_Curr_q_d_ref.qI_Component1=Stat_Curr_q_d_ref.qI_Component1-500;
		if(Stat_Curr_q_d_ref.qI_Component1<curqtemp)
		{Stat_Curr_q_d_ref.qI_Component1=curqtemp;}
	}

//	Stat_Curr_q_d_ref.qI_Component1=-1000;
	Stat_Curr_q_d_ref.qI_Component2 = 0;
}






























































